//
// Created by Administrator on 2021/7/31.
//

#include "Geometry/Quaternion.h"

#include <algorithm>

inline float Quaternion::Norm()
{
    return sqrtf(w() * w() + x() * x() + y() * y() + z() * z());
}

inline void Quaternion::Conjugate()
{
    x() = -x();
    y() = -y();
    z() = -z();
}

inline Quaternion Quaternion::Conjugated()
{
    return { w(), -x(), -y(), -z() };
}

inline void Quaternion::Invert()
{
    float norm = Norm();
    if(norm < 0.0001f) return;

    float invNorm = 1 / norm;
    x() = -x() * invNorm;
    y() = -y() * invNorm;
    z() = -z() * invNorm;
}

inline Quaternion Quaternion::Inverse()
{
    float norm = Norm();
    if(norm < 0.0001f) throw;

    float invNorm = 1 / norm;

    return {
        w(),
        -x() * invNorm,
        -y() * invNorm,
        -z() * invNorm
    };
}

Quaternion Quaternion::Log(const Quaternion& q)
{
    if(q.w() > 0.9999f) throw;

    float alpha = acosf(q.w());
    float sinA = sinf(alpha);
    float invSinA = 1 / sinA;

    return {
        0,
        alpha * (q.x() / invSinA),
        alpha * (q.y() / invSinA),
        alpha * (q.z() / invSinA)
    };
}

Quaternion Quaternion::Exp(const Quaternion& q)
{
    float alpha2 = q.x() * q.x() + q.y() * q.y() + q.z() * q.z();
    if(alpha2 < 0.0001) throw;

    float alpha = sqrtf(alpha2);
    float sinA = sinf(alpha);

    return {
        cosf(alpha),
        q.x() / alpha * sinA,
        q.y() / alpha * sinA,
        q.z() / alpha * sinA
    };
}

inline Quaternion Quaternion::Pow(const Quaternion& q, float exponent)
{
    if(q.w() > 0.9999f) return q;

    float alpha = acosf(q.w());
    float newAlpha = exponent * alpha;
    float mul = sinf(newAlpha) / sinf(alpha);
    float invMul = 1 / mul;
    return {
        cosf(newAlpha),
        q.x() * invMul,
        q.y() * invMul,
        q.z() * invMul};
}

inline Quaternion Quaternion::Slerp(const Quaternion& a, const Quaternion& b, const float& t)
{
    float w0 = a.w(), x0 = a.x(), y0 = a.y(), z0 = a.z();
    float w1 = b.w(), x1 = b.x(), y1 = b.y(), z1 = b.z();
    float k0, k1;
    float cosOmega = w0 * w1 + x0 * x1 + y0 * y1 + z0 * z1;
    // 如果为负值，反转一个Quaternion，保证夹角为最短弧
    if(cosOmega < 0)
    {
        w1 = -w1;
        x1 = -x1;
        y1 = -y1;
        z1 = -z1;
        cosOmega = -cosOmega;
    }
    // 如果夹角太小，可能会导致sin值特别小，导致除0，因此这种情况直接使用线性插值
    if(cosOmega > 0.9999f)
    {
        k0 = 1 - t;
        k1 = t;
    }
    else
    {
        // 勾股定理算sinOmega
        float sinOmega = sqrtf(1 - cosOmega * cosOmega);
        // atan2计算omega
        float omega = atan2f(sinOmega, cosOmega);
        // invSin
        float invSinOmega = 1 / sinOmega;
        // 计算k0
        k0 = sinf((1 - t) * omega) * invSinOmega;
        // 计算k1
        k1 = sinf(t * omega) * invSinOmega;
    }

    return {
        w0 * k0 + w1 * k1,
        x0 * k0 + x1 * k1,
        y0 * k0 + y1 * k1,
        z0 * k0 + z1 * k1,
    };
}